#include "BC_ContactTask.hpp"

template <typename T>
ContactTask<T>::ContactTask(){
    // param
    T length = 0.522;
    T width = 0.290;
    T high = 0;

    // RF
    _contact_data[0].OQ_init << length / 2.0, -width / 2.0, 0.0;
    _contact_data[0].OP_init << length / 2.0, -width / 2.0, high;

    //  LF
    _contact_data[1].OQ_init << length / 2.0, width / 2.0, 0.0;
    _contact_data[1].OP_init << length / 2.0, width / 2.0, high;

    // RB
    _contact_data[2].OQ_init << -length / 2.0, -width / 2.0, 0.0;
    _contact_data[2].OP_init << -length / 2.0, -width / 2.0, high;

    // LB
    _contact_data[3].OQ_init << -length / 2.0, width / 2.0, 0.0;
    _contact_data[3].OP_init << -length / 2.0, width / 2.0, high;
}

template <typename T>
void  ContactTask<T>::task(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, BCoutputData<T>* BCout, int leg){
    auto& seResult = data._stateEstimator->getResult();
    if(ctrl_data->contact_state[leg] > 0){
        Vec3<T> pDes, qDes, body_loc;
        pDes = data._legController->datas[leg].p;
        body_loc = seResult.position + data._quadruped->getHipLocation(leg);
        pDes(2) = -body_loc(2);
        computeLegAngle(*data._quadruped, pDes, &qDes);      
        BCout->qDes = qDes;
        BCout->qdDes = Vec3<T>::Zero();
    }
    else{
        BCout->qDes = data._legController->datas[leg].q;
        BCout->qdDes = data._legController->datas[leg].qd;       
    }
}

template <typename T>
void ContactTask<T>::calcurateLegData(T roll, T pitch, T yaw) {
    T err = 0.01;
    if (roll < err && roll > -err)
        roll = 0;
    if (pitch < err && pitch > -err)
        pitch = 0;
    if (yaw < err && yaw > -err)
        yaw = 0;
    T SR = sin(roll);
    T CR = cos(roll);
    T SP = sin(pitch);
    T CP = cos(pitch);
    T SY = sin(yaw);
    T CY = cos(yaw);
    Mat3<T> RX, RY, RZ;
    RX << 1, 0, 0, 0, CR, -SR, 0, SR, CR;
    RY << CP, 0, -SP, 0, 1, 0, SP, 0, CP;
    RZ << CY, -SY, 0, SY, CY, 0, 0, 0, 1;
    Rot = RX * RY * RZ;

    for (int leg = 0; leg < 4; leg++) {
        _contact_data[leg].OQ_now = Rot * _contact_data[leg].OQ_init;
        _contact_data[leg].QP_O = _contact_data[leg].OP_init - _contact_data[leg].OQ_now;
        _contact_data[leg].QP_Q = Rot.inverse() * _contact_data[leg].QP_O;
    }
}


template class ContactTask<float>;
template class ContactTask<double>;
